#include <rclcpp/rclcpp.hpp>
#include "svm_predictor.hpp"

class ForceMoveLogger : public rclcpp::Node
{
public:
  ForceMoveLogger()
  // : Node("force_move_logger"), latest_force_{0.3, 0.4, 0.5}
  : Node("force_move_logger"), latest_force_{1.5, 1.3, 1.5}
//   : Node("force_move_logger"), latest_force_{2.5, 2.0, 3.0}
  {
    classifier_ = std::make_unique<ForceClassifier>("/home/jetson/zc6/src/svm/force_svm_model.onnx");
  }

  void printInstantForce()
  {
    double Fx = latest_force_.fx;
    double Fy = latest_force_.fy;
    double Fz = latest_force_.fz;
    // === 标准化参数（从 Python 导出） ===
    static const double mean_x = 1.72200128, mean_y = 1.67677145, mean_z = 1.71118453;
    static const double scale_x = 1.11652727, scale_y = 1.12525363, scale_z = 1.09691274;

    // === 对输入力进行标准化 ===
    float Fx_n = (Fx - mean_x) / scale_x;
    float Fy_n = (Fy - mean_y) / scale_y;
    float Fz_n = (Fz - mean_z) / scale_z;

    // === 调用模型 ===
    int label = classifier_->predict(Fx_n, Fy_n, Fz_n);


    std::string category;
    if (label == 0) category = "hard";
    else if (label == 1) category = "medium";
    else if (label == 2) category = "soft";
    else category = "unknown";

    RCLCPP_INFO(this->get_logger(),
        "Fx=%.4f, Fy=%.4f, Fz=%.4f, 分类=%s",
        Fx, Fy, Fz, category.c_str());
  }

private:
  struct ForceValue { double fx, fy, fz; } latest_force_;
  std::unique_ptr<ForceClassifier> classifier_;
};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<ForceMoveLogger>();
  node->printInstantForce();
  rclcpp::shutdown();
  return 0;
}
